#include "lsm6dso.h"

static const char *TAG = "lsm6dso";
/**
 * @brief Init LSM6DSO
 * @param i2c_bus    {i2c_master_bus_handle_t} created by i2c_new_master_bus()
 * @param pObj pointer to lsm6dso object struct
 * @retval i2c_master_dev_handle_t: device handle of LSM6DSO
 */
esp_err_t lsm6dso_init(const i2c_master_bus_handle_t i2c_bus,i2c_master_dev_handle_t *dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t reg,temp;
    /* Pull up CS pin */
    ESP_RETURN_ON_ERROR(gpio_reset_pin(pObj->Pin.LSM6DSO_CS_PIN),TAG,"Reset CS pin failed");
    ESP_RETURN_ON_ERROR(gpio_set_direction(pObj->Pin.LSM6DSO_CS_PIN, GPIO_MODE_OUTPUT),TAG,"Failed to set GPIO output mode");
    ESP_RETURN_ON_ERROR(gpio_set_level(pObj->Pin.LSM6DSO_CS_PIN,1),TAG,"Failed to pull up CS pin");
    /* Add IMU to I2C Bus */
    i2c_device_config_t lsm6dso_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = pObj->Addr,
        .scl_speed_hz = pObj->SCL_Speed,
    };
    ESP_RETURN_ON_ERROR(i2c_master_bus_add_device(i2c_bus, &lsm6dso_cfg, dev_handle),TAG,"Install lsm6dso to I2C bus failed");
    /* Disable I3C */
    reg = LSM6DSO_CTRL9_XL;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(*dev_handle,&reg,&temp,1),TAG,"Failed to read CTRL9");
    temp = temp|(0b00000010);
    ESP_RETURN_ON_ERROR(lsm6dso_write_reg(*dev_handle,&reg,&temp),TAG,"Failed to write CTRL9");
    /* Enable BDU */
    reg = LSM6DSO_CTRL3_C;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(*dev_handle,&reg,&temp,1),TAG,"Failed to read CTRL3");
    temp = temp|(0b01000000);
    ESP_RETURN_ON_ERROR(lsm6dso_write_reg(*dev_handle,&reg,&temp),TAG,"Failed to write CTRL3");
    /* Power off Accelerometer and gyroscope */
    reg = LSM6DSO_CTRL1_XL;
    temp = (ODR_XL_OFF|FS_XL_2g);
    ESP_RETURN_ON_ERROR(lsm6dso_write_reg(*dev_handle,&reg,&temp),TAG,"Failed to write CTRL1");
    pObj->Acc_odr = ODR_XL_OFF;
    pObj->Acc_fs = FS_XL_2g;
    pObj->Acc_PowerMode = LSM6DSO_ACC_HIGH_PERFORMANCE_MODE;
    reg = LSM6DSO_CTRL2_G;
    temp = (ODR_G_OFF|FS_G_250dps);
    ESP_RETURN_ON_ERROR(lsm6dso_write_reg(*dev_handle,&reg,&temp),TAG,"Failed to write CTRL2");
    pObj->Gyro_odr = ODR_G_OFF;
    pObj->Gyro_fs = FS_G_250dps;
    pObj->Gyro_PowerMode = LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE;

    pObj->is_initialized = 1;

    return ESP_OK;
}
/**
 * @brief DeInit LSM6DSO
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO success deinit
 *         else: i2c_device deleted error(gpio_reset_pin always return OK)
 */
esp_err_t lsm6dso_deinit(i2c_master_dev_handle_t *dev_handle,LSM6DSO_Object_t *pObj)
{
    /* Set LSM6DSO CS Pin level to low*/
    gpio_reset_pin(pObj->Pin.LSM6DSO_CS_PIN);
    /* Change params in pObj */
    pObj->Acc_odr = ODR_XL_OFF;
    pObj->Gyro_odr = ODR_G_OFF;
    pObj->is_initialized = 0;
    /* Remove device handle from i2c bus */
    return i2c_master_bus_rm_device(*dev_handle);
}
/**
 * @brief Get LSM6DSO ID to check sensor work state
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @retval ESP_OK: LSM6DSO ID success match
 *         else: LSM6DSO ID doesn't match
 */
esp_err_t lsm6dso_get_id(const i2c_master_dev_handle_t dev_handle)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_WHO_AM_I;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read reg failed");
    if(temp==LSM6DSO_ID) return ESP_OK;
    else return ESP_ERR_INVALID_ARG;
}
esp_err_t lsm6dso_get_data(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Data_t *data,const LSM6DSO_Object_t *pObj)
{
    if(pObj->Acc_odr!=ODR_XL_OFF)
        ESP_RETURN_ON_ERROR(lsm6dso_acc_getaxes(dev_handle,&data->acc_raw,&data->acc,pObj),TAG,"Get accelerometer data failed");
    else
    {
        data->acc.x=0;
        data->acc.y=0;
        data->acc.z=0;
    }
    if(pObj->Gyro_odr!=ODR_G_OFF)
        ESP_RETURN_ON_ERROR(lsm6dso_gyro_getaxes(dev_handle,&data->gyro_raw,&data->gyro,pObj),TAG,"Get gyroscope data failed");
    else
    {
        data->gyro.x=0;
        data->gyro.y=0;
        data->gyro.z=0;
    }
    if(pObj->Acc_odr==ODR_XL_OFF && pObj->Gyro_odr==ODR_G_OFF) data->temp=0;
    else ESP_RETURN_ON_ERROR(lsm6dso_temp_getdata(dev_handle,&data->temp),TAG,"Get temperature data failed");
    ESP_RETURN_ON_ERROR(lsm6dso_timestamp_getdata(dev_handle,&data->timestamp),TAG,"Refresh timestamp failed");
    return ESP_OK;
}
/**
 * @brief Get Data from LSM6DSO's temperatrue sensor
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param temp_data pointer to temperatrue data buffer
 * @retval ESP_OK: LSM6DSO get temperatrue data success
 *         else: LSM6DSO get temperatrue data fail
 */
esp_err_t lsm6dso_temp_getdata(const i2c_master_dev_handle_t dev_handle,float_t *temp_data)
{
    uint8_t reg_temp_l = LSM6DSO_OUT_TEMP_L;
    uint8_t temp[2];
    int16_t temp_raw;
    uint8_t status_reg = LSM6DSO_STATUS_REG;
    uint8_t status = 0;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&status_reg,&status,1),TAG,"Read status reg failed");
    if(status & TEM_DATA_AVAILABLE)
    {
        /* Get Raw Data */
        ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_temp_l,temp,2),TAG,"Read temperature failed");
        temp_raw = temp[0]|temp[1]<<8;
        /* Calculate the Data */
        *temp_data = (float_t)temp_raw/256+25;
        return ESP_OK;
    }
    return ESP_ERR_INVALID_STATE;
}
esp_err_t lsm6dso_timestamp_setmode(const i2c_master_dev_handle_t dev_handle,bool mode,LSM6DSO_Object_t *pObj)
{
    uint8_t reg = LSM6DSO_CTRL10_C;
    uint8_t temp;
    if(mode==1) temp=0b00100000;
    else if(mode==0) temp=0;
    pObj->timestamp_mode=mode;
    return lsm6dso_write_reg(dev_handle,&reg,&temp);
}
esp_err_t lsm6dso_timestamp_getdata(const i2c_master_dev_handle_t dev_handle,uint32_t *timestamp)
{
    uint8_t reg = LSM6DSO_TIMESTAMP0;
    uint8_t temp[4];
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,temp,4),TAG,"Read timestamp reg failed");
    *timestamp =  (uint32_t)temp[3]<<24 | (uint32_t)temp[2]<<16 | (uint32_t)temp[1]<<8 | (uint32_t)temp[0];
    return ESP_OK;
}
/**
 * @brief Enable LSM6DSO Accelerometer, The default setting is 104Hz with 4g in High Performance Mode
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Accelerometer Enable success
 *         else: LSM6DSO Accelerometer Enable Failed
 */
esp_err_t lsm6dso_acc_enable(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL1_XL;
    temp = (ODR_XL_104Hz|FS_XL_4g);
    pObj->Acc_odr = ODR_XL_104Hz;
    pObj->Acc_fs = FS_XL_4g;
    return lsm6dso_write_reg(dev_handle, &reg, &temp);
}
/**
 * @brief Disable LSM6DSO Accelerometer, The default setting is ACC_OFF with 2g 
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Accelerometer Disabled success
 *         else: LSM6DSO Accelerometer Disabled Failed
 */
esp_err_t lsm6dso_acc_disable(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL1_XL;
    temp = (ODR_XL_OFF|FS_XL_2g);
    pObj->Acc_odr = ODR_XL_OFF;
    pObj->Acc_fs = FS_XL_2g;
    return lsm6dso_write_reg(dev_handle, &reg, &temp);
}
/**
 * @brief Get LSM6DSO's accelerometer powermode
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO's accelerometer powermode read success
 *         else: LSM6DSO's accelerometer powermode read fail
 */
esp_err_t lsm6dso_acc_getpowermode(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp,value;
    uint8_t reg1 = LSM6DSO_CTRL5_C, reg2 = LSM6DSO_CTRL6_C;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg2,&temp,1),TAG,"Read CTRL6 reg failed");
    value = temp<<3;
    value = temp>>4;
    if(value==1) pObj->Acc_PowerMode = LSM6DSO_ACC_LOW_POWER_NORMAL_MODE;
    else
    {
        ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg1,&temp,1),TAG,"Read CTRL5 reg failed");
        value = temp>>7;
        if(value==1) pObj->Acc_PowerMode = LSM6DSO_ACC_ULTRA_LOW_POWER_MODE;
        else pObj->Acc_PowerMode = LSM6DSO_ACC_HIGH_PERFORMANCE_MODE;
    }
    return ESP_OK;
}
/**
 * @brief Set LSM6DSO Accelerometer Power Mode
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param Mode Accelerometer Power Mode to set
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Accelerometer Set Power Mode success
 *         else: LSM6DSO Accelerometer Set Power Mode Failed
 */
esp_err_t lsm6dso_acc_setpowermode(const i2c_master_dev_handle_t dev_handle,LSM6DSO_ACC_Operating_Mode_t Mode,LSM6DSO_Object_t *pObj)
{
    switch (Mode)
    {
        case LSM6DSO_ACC_HIGH_PERFORMANCE_MODE:
        {
            uint8_t temp;
            uint8_t reg = LSM6DSO_CTRL6_C;
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL6 reg failed");
            //change XL_HM_MODE to 0
            temp = temp&(0b11101111);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg,&temp),TAG,"Write CTRL6 reg failed");
            pObj->Acc_PowerMode = LSM6DSO_ACC_HIGH_PERFORMANCE_MODE;
            return ESP_OK;
            break;
        }
        case LSM6DSO_ACC_LOW_POWER_NORMAL_MODE:
        {
            uint8_t temp;
            uint8_t reg = LSM6DSO_CTRL6_C;
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL6 reg failed");
            //change XL_HM_MODE to 1
            temp = temp|(0b00010000);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg,&temp),TAG,"Write CTRL6 reg failed");
            pObj->Acc_PowerMode = LSM6DSO_ACC_LOW_POWER_NORMAL_MODE;
            return ESP_OK;
            break;
        }
        case LSM6DSO_ACC_ULTRA_LOW_POWER_MODE:
        {
            uint8_t temp;
            uint8_t reg0 = LSM6DSO_CTRL2_G;
            uint8_t reg1 = LSM6DSO_CTRL1_XL;
            uint8_t reg2 = LSM6DSO_CTRL6_C;
            uint8_t reg3 = LSM6DSO_CTRL5_C;
            //Power down Gyroscope Sensor
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg0,&temp,1),TAG,"Read CTRL2 reg failed");
            temp = temp&(0b00001111);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg0,&temp),TAG,"Write CTRL2 reg failed");
            pObj->Gyro_odr = ODR_G_OFF;
            //Power down ACC before setting ACC to ULP MODE
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg1,&temp,1),TAG,"Read CTRL1 reg failed");
            temp = temp&(0b00001111);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg1,&temp),TAG,"Write CTRL1 reg failed");
            //change XL_HM_MODE to 0
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg2,&temp,1),TAG,"Read CTRL6 reg failed");
            temp = temp&(0b11101111);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg2,&temp),TAG,"Write CTRL6 reg failed");
            //change XL_ULP_MODE to 1
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg3,&temp,1),TAG,"Read CTRL5 reg failed");
            temp = temp|(0b10000000);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg3,&temp),TAG,"Write CTRL5 reg failed");
            //Restart ACC Sensor and set ACC ODR to 104Hz
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg1,&temp,1),TAG,"Read CTRL1 reg failed");
            temp = temp&(0b00001111);
            temp = temp|(ODR_XL_104Hz);
            ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg1,&temp),TAG,"Write CTRL1 reg failed");
            pObj->Acc_odr = ODR_XL_104Hz;
            pObj->Acc_PowerMode = LSM6DSO_ACC_ULTRA_LOW_POWER_MODE;
            return ESP_OK;
            break;
        }
        default:{
            return ESP_FAIL;
            break;
        }
    }
}
/**
 * @brief Get LSM6DSO's accelerometer ODR and Full scale
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO's accelerometer ODR and Full scale read success
 *         else: LSM6DSO's accelerometer ODR and Full scale read Failed
 */
esp_err_t lsm6dso_acc_getparams(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL1_XL;
    uint8_t value = 0;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL1 reg failed");
    //Get Sample Speed
    value = temp>>4;
    switch (value)
    {
    case 0:{
        pObj->Acc_odr = ODR_XL_OFF;
        break;
        }
    case 1:{
        pObj->Acc_odr = ODR_XL_12Hz5;
        break;
        }
    case 2:{
        pObj->Acc_odr = ODR_XL_26Hz;
        break;
        }
    case 3:{
        pObj->Acc_odr = ODR_XL_52Hz;
        break;
        }
    case 4:{
        pObj->Acc_odr = ODR_XL_104Hz;
        break;
        }
    case 5:{
        pObj->Acc_odr = ODR_XL_208Hz;
        break;
        }
    case 6:{
        pObj->Acc_odr = ODR_XL_416Hz;
        break;
        }
    case 7:{
        pObj->Acc_odr = ODR_XL_833Hz;
        break;
        }
    case 8:{
        pObj->Acc_odr = ODR_XL_1k66Hz;
        break;
        }
    case 9:{
        pObj->Acc_odr = ODR_XL_3k33Hz;
        break;
        }
    case 10:{
        pObj->Acc_odr = ODR_XL_6k66Hz;
        break;
        }
    case 11:{
        if(pObj->Acc_PowerMode == LSM6DSO_ACC_HIGH_PERFORMANCE_MODE) pObj->Acc_odr = ODR_XL_12Hz5;
        else pObj->Acc_odr = ODR_XL_1Hz6;
        break;
        }
    default:{
        return ESP_FAIL;
        break;
        }
    }
    /* Get Full Scale */
    //The code below only fit when XL_FS_MODE(CTRL8_XL) is 0(default value), check the datasheet for more details
    value = temp<<4;
    value = temp>>6;
    switch (value)
    {
    case 0:{
        pObj->Acc_fs = FS_XL_2g;
        break;
        }
    case 1:{
        pObj->Acc_fs = FS_XL_16g;
        break;
        }
    case 2:{
        pObj->Acc_fs = FS_XL_4g;
        break;
        }
    case 3:{
        pObj->Acc_fs = FS_XL_8g;
        break;
        }
    default:{
        return ESP_FAIL;
        break;
        }
    }
    return ESP_OK;
}
/**
 * @brief Set LSM6DSO Accelerometer ODR and Full Scale
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param SAMPLE_SPEED Accelerometer's ODR to set
 * @param FULL_SCALE Accelerometer's Full Scale to set
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Accelerometer Set ODR and Full Scale success
 *         else: LSM6DSO Accelerometer Set ODR and Full Scale Failed
 */
esp_err_t lsm6dso_acc_setparams(const i2c_master_dev_handle_t dev_handle,lsm6dso_odr_xl_t SAMPLE_SPEED,lsm6dso_fs_xl_t FULL_SCALE,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL1_XL;
    temp = (SAMPLE_SPEED|FULL_SCALE);
    ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg,&temp),TAG,"Write CTRL1 reg failed");
    pObj->Acc_odr = SAMPLE_SPEED;
    pObj->Acc_fs = FULL_SCALE;
    return ESP_OK;
}
/**
 * @brief Read LSM6DSO's Accelerometer Data
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param AxesRaw_t pointer to the struct which stored the raw data of the Accelerometer
 * @param Axes_t pointer to the struct which stored the processed data of the Accelerometer
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Accelerometer read data success
 *         else: LSM6DSO Accelerometer read data Failed
 */
esp_err_t lsm6dso_acc_getaxes(const i2c_master_dev_handle_t dev_handle,LSM6DSO_AxesRaw_t *AxesRaw_t,LSM6DSO_Axes_t *Axes_t,const LSM6DSO_Object_t *pObj)
{
    uint8_t reg_x_l = LSM6DSO_OUTX_L_A;
    uint8_t reg_y_l = LSM6DSO_OUTY_L_A;
    uint8_t reg_z_l = LSM6DSO_OUTZ_L_A;
    uint8_t temp[2];
    uint8_t status_reg = LSM6DSO_STATUS_REG;
    uint8_t status = 0;
    if(lsm6dso_read_reg(dev_handle,&status_reg,&status,1)!=ESP_OK) return ESP_FAIL;
    else
    {
        if(status & ACC_DATA_AVAILABLE)
        {
            //Get Raw Data
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_x_l,temp,2),TAG,"Read x axis acceleration failed");
            AxesRaw_t->x = temp[0]|temp[1]<<8;
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_y_l,temp,2),TAG,"Read y axis acceleration failed");
            AxesRaw_t->y = temp[0]|temp[1]<<8;
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_z_l,temp,2),TAG,"Read z axis acceleration failed");
            AxesRaw_t->z = temp[0]|temp[1]<<8;
            //Caculate the Data
            switch (pObj->Acc_fs)
            {
            case FS_XL_2g:{
                Axes_t->x = AxesRaw_t->x * 0.061;
                Axes_t->y = AxesRaw_t->y * 0.061;
                Axes_t->z = AxesRaw_t->z * 0.061;
                break;  
            }
            case FS_XL_4g:{
                Axes_t->x = AxesRaw_t->x * 0.122;
                Axes_t->y = AxesRaw_t->y * 0.122;
                Axes_t->z = AxesRaw_t->z * 0.122;
                break;  
            }
            case FS_XL_8g:{
                Axes_t->x = AxesRaw_t->x * 0.244;
                Axes_t->y = AxesRaw_t->y * 0.244;
                Axes_t->z = AxesRaw_t->z * 0.244;
                break;  
            }
            case FS_XL_16g:{
                Axes_t->x = AxesRaw_t->x * 0.488;
                Axes_t->y = AxesRaw_t->y * 0.488;
                Axes_t->z = AxesRaw_t->z * 0.488;
                break;  
            }
            default:
                break;
            }
            return ESP_OK;
        }
        return ESP_ERR_INVALID_STATE;
    }
}
/**
 * @brief Enable LSM6DSO Gyroscope, The default setting is 104Hz with 500dps in High Performance Mode
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Gyroscope Enable success
 *         else: LSM6DSO Gyroscope Enable Failed
 */
esp_err_t lsm6dso_gyro_enable(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL2_G;
    temp = (ODR_G_104Hz|FS_G_500dps);
    pObj->Gyro_odr = ODR_G_104Hz;
    pObj->Gyro_fs = FS_G_500dps;
    return lsm6dso_write_reg(dev_handle, &reg, &temp);
}
/**
 * @brief Disable LSM6DSO Gyroscope, The default setting is GYRO_OFF with 250dps 
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Gyroscope Disabled success
 *         else: LSM6DSO Gyroscope Disabled Failed
 */
esp_err_t lsm6dso_gyro_disable(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL2_G;
    temp = (ODR_G_OFF|FS_G_250dps);
    pObj->Gyro_odr = ODR_G_OFF;
    pObj->Gyro_fs = FS_G_250dps;
    return lsm6dso_write_reg(dev_handle, &reg, &temp);
}
/**
 * @brief Get LSM6DSO's Gyroscope powermode
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO's Gyroscope powermode read success
 *         else: LSM6DSO's Gyroscope powermode read fail
 */
esp_err_t lsm6dso_gyro_getpowermode(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL7_G;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL7 reg failed");
    if(temp>>7 == 0) pObj->Gyro_PowerMode = LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE;
    else pObj->Gyro_PowerMode = LSM6DSO_GYRO_LOW_POWER_NORMAL_MODE;
    return ESP_OK;
}
/**
 * @brief Set LSM6DSO Gyroscope Power Mode
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param Mode Gyroscope Power Mode to set
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Gyroscope Set Power Mode success
 *         else: LSM6DSO Gyroscope Set Power Mode Failed
 */
esp_err_t lsm6dso_gyro_setpowermode(const i2c_master_dev_handle_t dev_handle,LSM6DSO_GYRO_Operating_Mode_t Mode,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL7_G;
    switch (Mode)
    {
    case LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE:{
        ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL7 reg failed");
        temp = temp&(0b01111111);
        ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg,&temp),TAG,"Write CTRL7 reg failed");
        pObj->Gyro_PowerMode = LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE;
        return ESP_OK;
        break;
    }
    case LSM6DSO_GYRO_LOW_POWER_NORMAL_MODE:{
        ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL7 reg failed");
        temp = temp|(0b10000000);
        ESP_RETURN_ON_ERROR(lsm6dso_write_reg(dev_handle,&reg,&temp),TAG,"Write CTRL7 reg failed");
        pObj->Gyro_PowerMode = LSM6DSO_ACC_LOW_POWER_NORMAL_MODE;
        return ESP_OK;
        break;
    }
    default:{
        return ESP_FAIL;
        break;
    }
    }
}
/**
 * @brief Get LSM6DSO's Gyroscope ODR and Full scale
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO's Gyroscope ODR and Full scale read success
 *         else: LSM6DSO's Gyroscope ODR and Full scale read Failed
 */
esp_err_t lsm6dso_gyro_getparams(const i2c_master_dev_handle_t dev_handle,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL2_G;
    uint8_t value = 0;
    ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg,&temp,1),TAG,"Read CTRL2 reg failed");
    /* Get Sample Speed */
    value = temp>>4;
    switch (value)
    {
    case 0:
    {
        pObj->Gyro_odr = ODR_G_OFF;
        break;
        }
    case 1:
    {
        pObj->Gyro_odr = ODR_G_12Hz5;
        break;
        }
    case 2:
    {
        pObj->Gyro_odr = ODR_G_26Hz;
        break;
        }
    case 3:
    {
        pObj->Gyro_odr = ODR_G_52Hz;
        break;
        }
    case 4:
    {
        pObj->Gyro_odr = ODR_G_104Hz;
        break;
        }
    case 5:
    {
        pObj->Gyro_odr = ODR_G_208Hz;
        break;
        }
    case 6:
    {
        pObj->Gyro_odr = ODR_G_416Hz;
        break;
        }
    case 7:
    {
        pObj->Gyro_odr = ODR_G_833Hz;
        break;
        }
    case 8:
    {
        pObj->Gyro_odr = ODR_G_1k66Hz;
        break;
        }
    case 9:
    {
        pObj->Gyro_odr = ODR_G_3k33Hz;
        break;
        }
    case 10:
    {
        pObj->Gyro_odr = ODR_G_6k66Hz;
        break;
        }
    default:{
        return ESP_FAIL;
        break;
        }
    }
    /* Get Full Scale */
    value = temp<<4;
    value = temp>>6;
    switch (value)
    {
    case 0:{
        pObj->Gyro_fs = FS_G_250dps;
        break;
        }
    case 1:{
        pObj->Gyro_fs = FS_G_500dps;
        break;
        }
    case 2:{
        pObj->Gyro_fs = FS_G_1000dps;
        break;
        }
    case 3:{
        pObj->Gyro_fs = FS_G_2000dps;
        break;
        }
    default:{
        return ESP_FAIL;
        break;
        }
    }
    return ESP_OK;
}
/**
 * @brief Set LSM6DSO Gyroscope ODR and Full Scale
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param SAMPLE_SPEED Gyroscope's ODR to set
 * @param FULL_SCALE Gyroscope's Full Scale to set
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Gyroscope Set ODR and Full Scale success
 *         else: LSM6DSO Gyroscope Set ODR and Full Scale Failed
 */
esp_err_t lsm6dso_gyro_setparams(const i2c_master_dev_handle_t dev_handle,lsm6dso_odr_g_t SAMPLE_SPEED,lsm6dso_fs_g_t FULL_SCALE,LSM6DSO_Object_t *pObj)
{
    uint8_t temp;
    uint8_t reg = LSM6DSO_CTRL2_G;
    temp =  (SAMPLE_SPEED|FULL_SCALE);
    if(lsm6dso_write_reg(dev_handle,&reg,&temp)!=ESP_OK) return ESP_FAIL;
    pObj->Gyro_odr = SAMPLE_SPEED;
    pObj->Gyro_fs = FULL_SCALE;
    return ESP_OK;
}
/**
 * @brief Read LSM6DSO's Gyroscope Data
 * @param dev_handle {i2c_master_dev_handle_t} created by LSM6DSO_Init()
 * @param AxesRaw_t pointer to the struct which stored the raw data of the Gyroscope
 * @param Axes_t pointer to the struct which stored the processed data of the Gyroscope
 * @param pObj pointer to lsm6dso object struct
 * @retval ESP_OK: LSM6DSO Gyroscope read data success
 *         else: LSM6DSO Gyroscope read data Failed
 */
esp_err_t lsm6dso_gyro_getaxes(const i2c_master_dev_handle_t dev_handle,LSM6DSO_AxesRaw_t *AxesRaw_t,LSM6DSO_Axes_t *Axes_t,const LSM6DSO_Object_t *pObj)
{
    uint8_t reg_x_l = LSM6DSO_OUTX_L_G;
    uint8_t reg_y_l = LSM6DSO_OUTY_L_G;
    uint8_t reg_z_l = LSM6DSO_OUTZ_L_G;
    uint8_t temp[2];
    uint8_t status_reg = LSM6DSO_STATUS_REG;
    uint8_t status = 0;
    if(lsm6dso_read_reg(dev_handle,&status_reg,&status,1)!=ESP_OK) return ESP_FAIL;
    else
    {
        if(status & GYR_DATA_AVAILABLE)
        {
            /* Get Raw Data */
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_x_l,temp,2),TAG,"Read x axis angular velocity failed");
            AxesRaw_t->x = temp[0]|temp[1]<<8;
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_y_l,temp,2),TAG,"Read y axis angular velocity failed");
            AxesRaw_t->y = temp[0]|temp[1]<<8;
            ESP_RETURN_ON_ERROR(lsm6dso_read_reg(dev_handle,&reg_z_l,temp,2),TAG,"Read z axis angular velocity failed");
            AxesRaw_t->z = temp[0]|temp[1]<<8;
            /* Calculate the Data */
            switch (pObj->Gyro_fs)
            {
            case FS_G_250dps:{
                Axes_t->x = AxesRaw_t->x * 8.75;
                Axes_t->y = AxesRaw_t->y * 8.75;
                Axes_t->z = AxesRaw_t->z * 8.75;
                break;
            }
            case FS_G_500dps:{
                Axes_t->x = AxesRaw_t->x * 17.50;
                Axes_t->y = AxesRaw_t->y * 17.50;
                Axes_t->z = AxesRaw_t->z * 17.50;
                break;
            }
            case FS_G_1000dps:{
                Axes_t->x = AxesRaw_t->x * 35.0;
                Axes_t->y = AxesRaw_t->y * 35.0;
                Axes_t->z = AxesRaw_t->z * 35.0;
                break;
            }
            case FS_G_2000dps:{
                Axes_t->x = AxesRaw_t->x * 70.0;
                Axes_t->y = AxesRaw_t->y * 70.0;
                Axes_t->z = AxesRaw_t->z * 70.0;
                break;
            }
            default:
                break;
            }
            return ESP_OK;
        }
        return ESP_FAIL;
    }
}